// Quat.h: interface for the Quat class.
// A quaternion class. It can be used to represent an orientation in 3D space.
//////////////////////////////////////////////////////////////////////

#if !defined QUAT_H
#define QUAT_H

#include "Vec3.h"
#include "Vec4.h"
#include "Matrix.h"
#include<iostream>
using std::ostream;

class Quat
{

    public:
        // The only data member is a Vec4 which holds the elements
		Vec4 vec;	// a four-vector

		inline Quat(): vec(0.0f,0.0f,0.0f,1.0f) {}
		inline Quat( float x, float y, float z, float w ):  vec(x,y,z,w) {}
		inline Quat( const Vec4& v ): vec(v) {}

        inline Quat( float angle, const Vec3& axis)
        {
            makeRotate(angle, axis);
        }

        inline Quat( float angle1, const Vec3& axis1, 
                     float angle2, const Vec3& axis2,
                     float angle3, const Vec3& axis3)
        {
            makeRotate(angle1,axis1,angle2,axis2,angle3,axis3);
        }

		// methods

		inline Vec4& asVec4() {  return vec; }
		inline const Vec4& asVec4() const { return vec; }
		inline const Vec3 asVec3() const { return Vec3(vec[0], vec[1], vec[2]); }

        inline void set(float x, float y, float z, float w) { vec.set(x,y,z,w); }
        inline void set(const Vec4& v) { vec = v; }

        inline float& operator [] (int i) { return vec[i]; }
        inline float  operator [] (int i) const { return vec[i]; }

        inline float& x() { return vec[0]; }
        inline float& y() { return vec[1]; }
        inline float& z() { return vec[2]; }
        inline float& w() { return vec[3]; }

        inline float x() const { return vec[0]; }
        inline float y() const { return vec[1]; }
        inline float z() const { return vec[2]; }
        inline float w() const { return vec[3]; }

    	// return true if the Quat represents a zero rotation, and therefore can be ignored in computations.
    	bool zeroRotation() const { return vec[0]==0.0f && vec[1]==0.0f && vec[2]==0.0f && vec[3]==1.0f; } 

		// ARITHMETIC METHODS		    
		// Implemented in terms of Vec4s.  Some Vec4 operators, e.g. operator* are not appropriate for quaternions (as
		// mathematical objects) so they are implemented differently.
		// Also define methods for conjugate and the multiplicative inverse.		    

		// Multiply by scalar 
        inline const Quat operator * (float rhs) const { return Quat(vec*rhs); }

		// Unary multiply by scalar 
        inline Quat& operator *= (float rhs) { vec*=rhs; return *this; }

		// Binary multiply 
		inline const Quat operator*(const Quat& rhs) const
		{
			return Quat( rhs.vec[3]*vec[0] + rhs.vec[0]*vec[3] + rhs.vec[1]*vec[2] - rhs.vec[2]*vec[1],
			 rhs.vec[3]*vec[1] - rhs.vec[0]*vec[2] + rhs.vec[1]*vec[3] + rhs.vec[2]*vec[0],
			 rhs.vec[3]*vec[2] + rhs.vec[0]*vec[1] - rhs.vec[1]*vec[0] + rhs.vec[2]*vec[3],
			 rhs.vec[3]*vec[3] - rhs.vec[0]*vec[0] - rhs.vec[1]*vec[1] - rhs.vec[2]*vec[2] );
		}

		// Unary multiply 
		inline Quat& operator*=(const Quat& rhs)
		{
			float x = rhs.vec[3]*vec[0] + rhs.vec[0]*vec[3] + rhs.vec[1]*vec[2] - rhs.vec[2]*vec[1];
			float y = rhs.vec[3]*vec[1] - rhs.vec[0]*vec[2] + rhs.vec[1]*vec[3] + rhs.vec[2]*vec[0];
			float z = rhs.vec[3]*vec[2] + rhs.vec[0]*vec[1] - rhs.vec[1]*vec[0] + rhs.vec[2]*vec[3];
			vec[3]   = rhs.vec[3]*vec[3] - rhs.vec[0]*vec[0] - rhs.vec[1]*vec[1] - rhs.vec[2]*vec[2];

			vec[2] = z;
			vec[1] = y;
			vec[0] = x;

			return (*this);			// enable nesting
		}

		// Divide by scalar 
        inline const Quat operator / (float rhs) const { return Quat(vec/rhs); }

		// Unary divide by scalar 
        inline Quat& operator /= (float rhs) { vec/=rhs; return *this; }

		// Binary divide 
		inline const Quat operator/(const Quat& denom) const {
			return ( (*this) * denom.inverse() );}

		// Unary divide 
		inline Quat& operator/=(const Quat& denom){
			(*this) = (*this) * denom.inverse();
			return (*this);			// enable nesting
		}

		// Binary addition 
		inline const Quat operator + (const Quat& rhs) const {
            return Quat( vec + rhs.vec ); }

		// Unary addition
        inline Quat& operator += (const Quat& rhs){
            vec += rhs.vec;
            return *this;			// enable nesting
        }

        /// Binary subtraction 
        inline const Quat operator - (const Quat& rhs) const
        {
            return Quat( vec - rhs.vec );
        }

        /// Unary subtraction 
        inline Quat& operator -= (const Quat& rhs)
        {
            vec-=rhs.vec;
            return *this;			// enable nesting
        }

        // Negation operator - returns the negative of the quaternion.
        inline const Quat operator - () const { return Quat ( -vec ); }

        /// Length of the quaternion = sqrt( vec . vec )
        float length() const { return vec.length(); }

        /// Length of the quaternion = vec . vec
        float length2() const { return vec.length2(); }
        
		/// Conjugate 
        inline Quat conj () const { 
			return Quat( -vec[0], -vec[1], -vec[2], vec[3] ); }

        /// Multiplicative inverse method: q^(-1) = q^*/(q.q^*)
        inline const Quat inverse () const {
             return conj() / length2(); }


		// METHODS RELATED TO ROTATIONS
		// Set a quaternion which will perform a rotation of an angle around the axis given by 
		// the vector (x,y,z). 

        void makeRotate( float angle, float x, float y, float z );

        void makeRotate ( float angle, const Vec3& vec );

        void makeRotate ( float angle1, const Vec3& axis1, 
                          float angle2, const Vec3& axis2,
                          float angle3, const Vec3& axis3);

        // Make a rotation Quat which will rotate vec1 to vec2. Generally take a dot product to get 
		// the angle between these and then use a cross product to get the rotation axis Watch out for 
		// the two special cases of when the vectors are co-incident or opposite in direction.

        void makeRotate( const Vec3& vec1, const Vec3& vec2 );
    
        // Return the angle and vector components represented by the quaternion.
        void getRotate ( float& angle, float& x, float& y, float& z ) const;

        // Return the angle and vector represented by the quaternion.*/
        void getRotate ( float& angle, Vec3& vec ) const;

        // Spherical Linear Interpolation.
	    // As t goes from 0 to 1, the Quat object goes from "from" to "to". 
        void slerp ( float t, const Quat& from, const Quat& to);
        
        // Set quaternion to be equivalent to specified matrix.
        void set( const Matrix& m );

        // Get the equivalent matrix for this quaternion.
        void get( Matrix& m ) const;

        // Get the equivalent matrix for this quaternion.
        Matrix getMatrix() const
        {
            Matrix matrix;
            get(matrix);
            return matrix;
        }
        
        friend inline std::ostream& operator << (std::ostream& output, const Quat& vec)
        {
            output  << "Q(" << vec.vec[0] << "," << vec.vec[1] << "," << vec.vec[2] << "," << vec.vec[3] << ")";
            return output; 	
        }

};	// end of class prototype

#endif // QUAT_H
